package com.service.impl;

import com.entity.FlightRealTimeData;
import com.common.enums.PX4CustomMainMode;
import com.common.enums.PX4CustomSubModeAuto;
import com.mapper.influx.FlightRealTimeDataMapper;
import com.service.DroneService;
import com.utils.DroneConnection;
import com.websocket.MavlinkMessageHandler;
import io.dronefleet.mavlink.MavlinkConnection;
import io.dronefleet.mavlink.common.*;
import io.dronefleet.mavlink.minimal.*;
import io.dronefleet.mavlink.util.EnumValue;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

import javax.annotation.PostConstruct;
import java.io.IOException;
import java.net.SocketException;
import java.util.List;
import java.util.concurrent.CompletableFuture;
import java.util.concurrent.ExecutionException;

import static com.MAVLink.common.msg_attitude.MAVLINK_MSG_ID_ATTITUDE;
import static com.MAVLink.common.msg_home_position.MAVLINK_MSG_ID_HOME_POSITION;
import static com.MAVLink.common.msg_local_position_ned.MAVLINK_MSG_ID_LOCAL_POSITION_NED;
import static com.MAVLink.common.msg_vfr_hud.MAVLINK_MSG_ID_VFR_HUD;

/**
 * @className: DroneServiceImpl
 * @author: Kuangkeli
 * @date: 2024/07/01 15:44
 **/
@Service
public class DroneServiceImpl implements DroneService  {

    @Autowired
    private static MavlinkMessageHandler mavlinkMessageHandler;

    @Autowired
    private FlightRealTimeDataMapper flightRealTimeDataMapper;

    /**
     * 定义目标系统号
     */
    private static int systemId = 1;

    /**
     * 定义目标组件号
     */
    private static int componentId = 1;

    @PostConstruct
    public void init() throws IOException {
        //请求指令
       requestLocalPositionNed();
       requestAttitude();
       requestVFR_HUD();
    }

    @Override
    public void setAutoTakeoffMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF.getValue() << 24))
                .build();
       connection.send2(1, 1, setMode);
    }

    @Override
    public void setOffBoardMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(systemId)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((long) PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_OFFBOARD.getValue() << 16)
                .build();
        // 发送SET_MODE消息
        connection.send2(systemId, componentId, setMode);
    }

    @Override
    public void sendLandCommand() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        CommandLong landCommand = CommandLong.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .command(MavCmd.MAV_CMD_NAV_LAND)
                .build();
        connection.send2(systemId, componentId, landCommand);
    }

    @Override
    public void sendHeartbeat() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        Heartbeat heartbeat = Heartbeat.builder()
                .type(MavType.MAV_TYPE_QUADROTOR)
                .autopilot(MavAutopilot.MAV_AUTOPILOT_PX4)
                .systemStatus(MavState.MAV_STATE_STANDBY)
                .mavlinkVersion(3)
                .build();
        // Write an unsigned heartbeat
        connection.send2(systemId, componentId, heartbeat);
    }

    @Override
    public void requestLocalPositionNed() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        RequestDataStream requestDataStream = RequestDataStream.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .reqStreamId(MAVLINK_MSG_ID_LOCAL_POSITION_NED)
                .reqMessageRate(5)
                .startStop(1)
                .build();
        connection.send2(systemId, componentId, requestDataStream);
    }

    @Override
    public HomePosition requestHomePosition() throws IOException, InterruptedException {
        MavlinkConnection connection = DroneConnection.getInstance();
        CommandLong getHomePositionCommand = CommandLong.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .command(MavCmd.MAV_CMD_REQUEST_MESSAGE)
                .param1(MAVLINK_MSG_ID_HOME_POSITION)
                .build();
        connection.send2(systemId,componentId,getHomePositionCommand);
        HomePosition homePosition = null;
        try {
            // 阻塞等待结果
            homePosition = getHomePositionAsync().get();
            System.out.println(homePosition);
        } catch (InterruptedException | ExecutionException e) {
            e.printStackTrace();
        }
        return homePosition;
    }

    public static CompletableFuture<HomePosition> getHomePositionAsync() {
        return CompletableFuture.supplyAsync(() -> {
            try {
                Thread.sleep(100);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
            return mavlinkMessageHandler.getHomePosition(); // 异步获取homePosition
        });
    }

    @Override
    public void requestAttitude() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        RequestDataStream requestDataStream = RequestDataStream.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .reqStreamId(MAVLINK_MSG_ID_ATTITUDE)
                .reqMessageRate(5)
                .startStop(1)
                .build();
        connection.send2(systemId, componentId, requestDataStream);
    }

    @Override
    public void setAutoReturnMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_RTL.getValue() << 24))
                .build();
        connection.send2(systemId, componentId, setMode);
    }

    @Override
    public void setAutoMissionMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_MISSION.getValue() << 24))
                .build();
        connection.send2(systemId, componentId, setMode);
    }

    @Override
    public void setAutoLoiterMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_LOITER.getValue() << 24))
                .build();
        connection.send2(systemId, componentId, setMode);
    }

    @Override
    public void setAutoLandMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_LAND.getValue() << 24))
                .build();
        connection.send2(systemId, componentId, setMode);
    }

    @Override
    public void requestVFR_HUD() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        RequestDataStream requestDataStream = RequestDataStream.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .reqStreamId(MAVLINK_MSG_ID_VFR_HUD)
                .reqMessageRate(5)
                .startStop(1)
                .build();
        connection.send2(systemId, componentId, requestDataStream);
    }

    @Override
    public void sendflyRectangleCmdOnOffBoard(float length, float width, float current_x, float current_y, float flightAltitude, boolean clockwise,float cycle_s) throws InterruptedException, IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        // 飞行高度
        float flight_altitude = -(float)flightAltitude ;

        // 轨迹信息发送频率
        float rate = 20.0f;

        // 完成一圈矩形的时间
        float cycles = (float) cycle_s;

        int steps = (int) (cycles * rate);

        float dt = 1.0f / rate;

        int timeNowMs = 0;
        int i = 0;

        // 确定矩形的四个顶点
        float[][] points = {
                {0, 0},
                {length, 0},
                {length, width},
                {0, width}
        };

        if (!clockwise) {
            // 如果逆时针飞行，反转顶点顺序
            points = new float[][]{
                    {0, 0},
                    {0, width},
                    {length, width},
                    {length, 0}
            };
        }

        while (i < steps) { // Corrected loop condition to execute only one loop
            // 计算当前线段的起点和终点
            int segment = (i / (steps / 4)) % 4;
            float[] start = points[segment];
            float[] end = points[(segment + 1) % 4];

            // 计算当前点在起点和终点之间的插值
            float t = (float) (i % (steps / 4)) / (steps / 4);
            float x = start[0] + t * (end[0] - start[0]);
            float y = start[1] + t * (end[1] - start[1]);

            // 计算当前飞行方向的偏航角（弧度）
            float yaw = (float) Math.atan2(end[1] - start[1], end[0] - start[0]);

            i++;

            // 创建目标位置消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs((int) (timeNowMs))
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE
                    )
                    .x(current_x + x)
                    .y(current_y + y)
                    .z(flight_altitude)
                    .vx(0)
                    .vy(0)
                    .afx(0)
                    .afy(0)
                    .yaw(yaw)
                    .build();
            connection.send2(systemId, componentId, targetLocal);
            System.out.println("x:" + x + "   y:" + y + "   yaw:" + yaw);
            timeNowMs += 1000 / rate;
            // 等待一段时间
            Thread.sleep((long) (1000 / rate));
        }
    }

    @Override
    public void sendflyCircleCmdOnOffBorad(float radius, boolean clockwise,float current_x, float current_y, float flightAltitude, float cycle_s,boolean isUp) throws IOException, InterruptedException {
        MavlinkConnection connection = DroneConnection.getInstance();
        // 飞行高度
        float flight_altitude = -(float) flightAltitude;

        // 轨迹信息发送频率
        float rate = 20.0f;

        // 完成一圈圆形的时间
        float cycles = (float) cycle_s;

        int steps = (int) (cycles * rate);

        float dt = 1.0f / rate;

        int timeNowMs = 0;
        int i = 0;

        int directionFactor = clockwise ? 1 : -1; // 顺时针为1，逆时针为-1
        int up = isUp ? -1 : 1;
        while (i < steps) {
            // 计算当前位置的参数
            float t = i * dt;
            float angle = up * directionFactor * 2.0f * (float) Math.PI * t / cycle_s;
            float x = radius * (float) Math.cos(angle);
            float y = radius * (float) Math.sin(angle);
            float yaw = (clockwise) ? angle + (float) Math.PI / 2.0f: angle - (float) Math.PI / 2.0f;

            float task_x = current_x + x - radius;
            float task_y = current_y + y;
            if(isUp){
                task_x = 2 * current_x - task_x;
                yaw = -yaw;
            }
            // 创建目标位置消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs(timeNowMs)
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
                    .x(task_x)
                    .y(task_y)
                    .z(flight_altitude)
                    .yaw(yaw)
                    .build();

            // 发送目标位置消息
            connection.send2(systemId, componentId, targetLocal);

            System.out.println("x:" + (task_x) + "   y:" + (task_y) + "   yaw:" + yaw);
            timeNowMs += 1000 / rate;

            // 等待一段时间
            Thread.sleep((long) (1000 / rate));
            i++;
        }
    }

    @Override
    public void flyPointToPoint(float startX, float startY, float endX, float endY, float flightAltitude, float duration_s) throws InterruptedException, IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        // 飞行高度
        float flight_altitude = -(float) flightAltitude;

        // 轨迹信息发送频率
        float rate = 20.0f;

        // 完成飞行的时间（秒）
        float durations = (float) duration_s;

        int steps = (int) (durations * rate);

        float dt = 1.0f / rate;

        int timeNowMs = 0;


        for (int i = 0; i < steps; i++) {
            // 计算当前点在起点和终点之间的插值
            float t = (float) i / steps;
            float x = startX + t * (endX - startX);
            float y = startY + t * (endY - startY);

            // 计算当前飞行方向的偏航角（弧度），头朝前飞行
            float yaw = (float) Math.atan2(endY - startY, endX - startX);

            // 创建目标位置消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs((int) (timeNowMs))
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
                    .x(x)
                    .y(y)
                    .z(flight_altitude)
                    .yaw(yaw)
                    .build();
            connection.send2(systemId, componentId, targetLocal);

            timeNowMs += 1000 / rate;

            // 等待一段时间
            Thread.sleep((long) (1000 / rate));
        }

    }

    @Override
    public void flyRectangleByP2P(float flightAltitude, float endX, float endY, float current_x, float current_y, boolean clockwise, float duration_s) throws InterruptedException, IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        // 飞行高度
        float flight_altitude = (float) flightAltitude;
        float width = Math.abs(endX - current_x);
        float height = Math.abs(endY - current_y);
        float perimeter =  (width + height);
        float height_time = (duration_s / 2) * (height / perimeter) ;
        float width_time = (duration_s / 2) * (width / perimeter) ;

         float[]  times =  {};

         times  = new float[]{height_time, width_time, height_time, width_time};

        int timeNowMs = 0;

        float yaw =  (float) Math.toRadians(90);

        if (!clockwise) {
            yaw = -yaw;
            times  = new float[]{width_time,height_time,width_time, height_time};
        }

        // 确定矩形的四个顶点
        float[][] points = {
                {current_x, current_y},
                {endX, current_y},
                {endX, endY},
                {current_x, endY}
        };

        if (!clockwise) {
            // 如果逆时针飞行，反转顶点顺序
            points = new float[][]{
                    {current_x, current_y},
                    {current_x, endY},
                    {endX, endY},
                    {endX, current_y}
            };
        }

        for (int i = 0; i < 4; i++) {
            int nextIndex = (i + 1) % 4;
            System.out.printf("from %f,%f to %f,%f \n", points[i][0], points[i][1], points[nextIndex][0], points[nextIndex][1]);
            flyPointToPoint( points[i][0], points[i][1], points[nextIndex][0], points[nextIndex][1],flight_altitude,times[nextIndex]);

            Thread.sleep(2000);
            // 创建目标位置消息，只在原地旋转90度
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs((int) (timeNowMs))
                    .targetSystem(systemId)
                    .targetComponent(componentId)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_X_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_Y_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_Z_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE
                    )
                    .x(points[nextIndex][0])
                    .y(points[nextIndex][1])
                    .z(flight_altitude)
                    .vx(0)
                    .vy(0)
                    .afx(0)
                    .afy(0)
                    .yaw(yaw)  // 旋转90度
                    .yawRate(5.0f)
                    .build();
            connection.send2(systemId, componentId, targetLocal);
            Thread.sleep(2000);
        }
    }

    /**
     * 通过点到点执行飞行矩形轨迹
     *
     * @param timeId
     */
    @Override
    public List<FlightRealTimeData> getFlightRealDataListByTimeId(String timeId) {
        return flightRealTimeDataMapper.select(timeId);
    }

    /**
     * 查询全部TimeId
     */
    @Override
    public List<String> getAllTimeId() {
        return flightRealTimeDataMapper.getAllRecords();
    }

    /**
     * 摇杆控制
     *
     * @param x
     * @param y
     * @param z
     * @param r
     */
    @Override
    public void remoteControl(int x, int y, int z, int r) throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        ManualControl manualControl = ManualControl.builder()
                .x(y)
                .y(x)
                .z(z)
                .r(r)
//                .buttons(65535)
//                .buttons2(65535)
                .target(1)
                .build();
        // Send the message
        connection.send2(systemId, componentId, manualControl);
    }

    /**
     * 上锁飞机
     *
     */
    @Override
    public void disArm() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        CommandLong commandLong = CommandLong.builder()
                .command(MavCmd.MAV_CMD_COMPONENT_ARM_DISARM)
                .param1(0)
                .targetSystem(1)
                .targetComponent(1)
                .build();
        // Send the message
        connection.send2(systemId, componentId, commandLong);
    }
}